#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <bitset>

using namespace std;

uint16_t CRC16_CHECK(uint8_t Data, uint16_t CRC_Init) {
  static uint16_t CRC_REG;
  static uint8_t move_num;
  CRC_REG = (CRC_Init & 0xFF00) | (Data ^ (CRC_Init & 0x00FF));
  for (move_num = 1; move_num < 9; move_num++) {
    if (CRC_REG & 0x0001) {
      CRC_REG = (CRC_REG >> 1) ^ 0xA001;
    } else {
      CRC_REG = CRC_REG >> 1;
    }
  }
  return CRC_REG;
}

uint16_t CRC_CHECK(std::vector<uint8_t> RX, size_t LEN) {
  uint16_t crc_result = 0xFFFF;
  uint16_t crc_high, crc_low;
  for (int i = 0; i < LEN; i++) {
    crc_result = CRC16_CHECK(RX[i], crc_result);
  }

  crc_low = (crc_result >> 8) & 0XFF;
  crc_high = crc_result & 0XFF;
  crc_result = crc_high << 8 | crc_low;

  return crc_result;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "test");
  ros::NodeHandle n;

  ros::Rate loop_rate(10);

  std::vector<uint8_t> input;
  input.clear();
  input.resize(6);
  input[0] = 0x01;
  input[1] = 0x03;
  input[2] = 0x00;
  input[3] = 0x0E;
  input[4] = 0x00;
  input[5] = 0x01;
  size_t z = input.size();

  uint16_t output = CRC_CHECK(input, z);
  cout << output << endl;

  int a = 300;
  cout << hex << a << dec << endl;
  uint8_t b1, b2;
  b1 = static_cast<uint8_t>(a);
  cout << __LINE__ << ":" << dec << (int)b1 << endl;
  cout << "(a>>8):" << (a >> 8) << endl;
  b2 = static_cast<uint8_t>(a >> 8);
  cout << "b2:" <<(int)b2<< endl;


  int fushu = -100;
  cout << "FuShu: " << bitset<16>(fushu) << endl;
  std::vector<uint8_t> msg;
  msg.clear();
  msg.resize(6);
  msg[0] = 0x01;
  msg[1] = 0x06;
  msg[2] = 0x00;
  msg[3] = 0x02;
  msg[4] = static_cast<uint8_t>(fushu >> 8);
  msg[5] = static_cast<uint8_t>(fushu);
  for (int k = 0; k < msg.size(); k++)
  {
      printf("%0x ", msg[k]);
  }
  printf("\r\n");

  
//   cout << __LINE__ << ":" << dec << b2 << (int)'d' << endl;
  //   cout << __LINE__ << ":" << hex << b1 << endl;
  //   cout << __LINE__ << ":" << hex << b2 << endl;

  int count = 0;
  while (ros::ok()) {
    // cout << "hlllllllll" << endl;
    ros::spinOnce();
    loop_rate.sleep();
    ++count;
  }

  return 0;
}
